import ROSConnection from "./ROSConnection"

let ros = null
let commandTopic = null

function sendCommand(cmd_id, params = {}) {
  if (!commandTopic) return
  const now = Date.now() * 1000
  const msg = {
    timestamp: now,
    command: cmd_id,
    target_system: 1,
    target_component: 1,
    source_system: 1,
    source_component: 1,
    confirmation: 0,
    from_external: true,
    param1: params.param1 || 0,
    param2: params.param2 || 0,
    param3: params.param3 || 0,
    param4: params.param4 || 0,
    param5: params.param5 || 0,
    param6: params.param6 || 0,
    param7: params.param7 || 0
  }
  commandTopic.publish(msg)
  // console.log('[COMMAND]', msg)
}

export default {
  //指令发布
  init(rosInstance) {
    ros = rosInstance
    commandTopic = new ROSLIB.Topic({
      ros: ros,
      name: '/fmu/in/vehicle_command',
      messageType: 'px4_msgs/msg/VehicleCommand'
    })

  },
  sendFlightMode(modeInt = 6) {
    sendCommand(176, { param1: 1, param2: modeInt }) // MAV_CMD_DO_SET_MODE
  },
  arm() {
    sendCommand(400, { param1: 1 }) // ARM
  },
  disarm() {
    sendCommand(400, { param1: 0 }) // DISARM
  },
  takeoff(alt = 20) {
    // sendCommand(84, {param7: alt }) // TAKEOFF
    // 先切多旋翼模式
    sendCommand(176, { param1: 1.0, param2: 1.0 }) //MC_MODE   acro?
    setTimeout(() => {
      sendCommand(84, { param7: alt }) // MAV_CMD_NAV_VTOL_TAKEOFF
    }, 2000);

    // setInterval(() => {
    //   if (ROSConnection.getCurrentAlt() >= 19.0) {
        sendCommand(100001, { param1: 4 }) // MAV_CMD_DO_SET_MODE MAV_MODE_FLAG_CUSTOM_MODE_ENABLED HOLD模式=5
    //     // sendCommand(2147483648, { param1: 3 }) // MAV_CMD_DO_SET_MODE MAV_MODE_FLAG_CUSTOM_MODE_ENABLED HOLD模式=5
        // sendCommand(17, { param3: 50 }) 
    //     sendCommand(3000, { param1: 1.0 }) //FW 固定翼模式
        
    //     // setTimeout(() => {
    //     //   sendCommand(176, { param1: 1.0, param2: 5.0 }) // 禁用自动切固定翼
    //     // }, 2000);
    //     // setTimeout(() => {
    //     //   sendCommand(180, { param1: ROSConnection.hashParamName('VT_FW_MIN_ALT'), param2: 9999.0 }) // 禁用自动切固定翼
    //     // }, 2000);

    //   }
    // }, 1000)
  },
  // hold() {
  //   sendCommand(34,{param1:20,param2:5,param3:1,param5:NaN,param7: 30}) // hold
  // },
  // hold() {
  //   sendCommand(19,{param1:30,param2:0,param3:10,param4:0,param5:NaN,param6:NaN,param7: NaN}) // hold
  // },
  // hold() {
  //   sendCommand(17,{param1:217,param2:5,param3:0,param4:0,param5:0,param6:0,param7: 0}) // hold
  // },
  hold() {
    // sendCommand(17, { param1: 0, param2: 0, param3: 20, param4: 0, param5: NaN, param6: NaN, param7: NaN }) // hold
    sendCommand(100001, { param1: 4 }) // hold
  },
  land() {
    sendCommand(21) // LAND
  },
  goto(lat, lon, alt) {
    sendCommand(16, { param5: lat, param6: lon, param7: alt }) // WAYPOINT
  }

}
